import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import launch_ros.parameter_descriptions

def generate_launch_description():
    # 获取默认路径
    urdf_package_path = get_package_share_directory('fur_description')
    default_model_path = urdf_package_path + '/urdf/fur.urdf.xacro' 
    default_rviz_config_path = urdf_package_path + '/config/display_robot_model.rviz'

    # 声明一个urdf参数方便修改
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model_path', default_value=str(default_model_path),
        description='加载的模型URDF文件的绝对路径'
    )

    # gui参数
    action_declare_gui_arg = launch.actions.DeclareLaunchArgument(name='gui', default_value='true', choices=['true', 'false'],
                                    description='Flag to enable joint_state_publisher_gui')

    # 通过文件路径获取内容，并转换成参数值对象，传递给robot_state_publihser
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['xacro ',launch.substitutions.LaunchConfiguration('model_path')]
        ),
        value_type=str
    )

    # 状态发布节点
    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )

    # 关节状态发布节点
    action_joint_state_publisher = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        condition=launch.conditions.UnlessCondition(launch.substitutions.LaunchConfiguration('gui'))
    )

    # gui节点
    action_joint_state_publisher_gui_node = launch_ros.actions.Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        condition=launch.conditions.IfCondition(launch.substitutions.LaunchConfiguration('gui'))
    )

    # RViz节点
    action_rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', default_rviz_config_path]
    )

    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_declare_gui_arg,
        action_robot_state_publisher,
        action_joint_state_publisher,
        action_joint_state_publisher_gui_node,
        action_rviz_node
    ])